/**
 * 
 */
package edu.caltech.android.gaussianMixture;

import java.util.Iterator;
import java.util.Vector;

import Jama.Matrix;
import Jama.SingularValueDecomposition;
import edu.caltech.android.sensor.accel.AccelSample;
import edu.emory.mathcs.jtransforms.fft.DoubleFFT_1D;

/**
 * Mathematical functions used in anomaly detection.
 */
public final class MathLib {

	private MathLib() {
	}

	/**
	 * Gets the sample covariance matrix.
	 * 
	 * @param data
	 *            Each Matrix is another piece of data given as a column vector.
	 * @return The sample covariance matrix.
	 */
	public static Matrix covariance(final Vector<Matrix> data) {
		Matrix mean = MathLib.moment(data, 1);
		int dim = mean.getRowDimension();
		// Sum of (X - E[X])(X - E[X])'
		Matrix sumMults = new Matrix(dim, dim, 0);
		Iterator<Matrix> iter = data.iterator();
		while (iter.hasNext()) {
			Matrix m = iter.next();
			m.minusEquals(mean);
			m = m.times(m.transpose());
			sumMults.plusEquals(m);
		}
		// Divide by the number of samples
		int numSamples = data.size();
		sumMults.timesEquals(1 / ((double) numSamples));
		return sumMults;
	}

	/**
	 * Computes the DFFT of the x, y, and z-values of the given measurements.
	 * 
	 * @param measurements
	 *            Vector of measurements (as vectors) for which to take the
	 *            DFFT. Assumes that the measurements are relatively
	 *            evenly-spaced in time, which they almost certainly aren't, but
	 *            it probably doesn't that much and there's nothing that we can
	 *            really do about that.
	 * @param numFFT
	 *            Number of Fourier coefficients to get. Note that if 'numFFT'
	 *            is greater than the number of measurements, then the
	 *            measurements will be extended with 0s to have size 'numFFT'.
	 *            If 'numFFT' is smaller than the number of measurements, then
	 *            only the first 'numFFT' measurements are used in getting the
	 *            FFT coefficients.
	 * @return Column vector of Fourier coefficients, alternating between the x,
	 *         y, and z coefficients.
	 */
	public static Matrix dfft(final Vector<Matrix> measurements,
			final int numFFT) {
		// Number of measurements to actually use
		final int numUsed = Math.min(numFFT, measurements.size());

		final int numPoints = measurements.size();

		// Dimension of each point
		final int mDim = measurements.get(0).getRowDimension();

		// Put all of our data into double arrays
		// Go through x, y, and z. We need to allocate this much because
		// we need to make room for the complex component once we do the FFT
		final double[][] data = new double[mDim][2 * numFFT];
		for (int j = 0; j < mDim; j++) {
			for (int i = 0; i < numUsed; i++) {
				data[j][i] = measurements.get(i).get(j, 0);
			}
		}
		// Pad with zeros, if necessary.
		for (int j = 0; j < mDim; j++) {
			for (int i = numUsed; i < numFFT; i++) {
				data[j][i] = 0;
			}
		}

		// Do the FFTs
		final DoubleFFT_1D fft = new DoubleFFT_1D(numFFT);
		for (int i = 0; i < mDim; i++) {
			fft.realForwardFull(data[i]);
		}

		// Now put the absolute values of the coefficients in the output matrix
		// Store them in a single column in the following order:
		// fftx1, ffty1, fftz1, fftx2, ffty2, fftz2, fftx3, ...
		final Matrix fftCoeffs = new Matrix(numFFT * mDim, 1);
		for (int i = 0; i < mDim; i++) {
			for (int j = 0; j < numFFT; j++) {
				final double real = data[i][2 * j];
				final double complex = data[i][2 * j + 1];
				final double mag = Math.sqrt(real * real + complex * complex);
				// Divide by the number of points.
				// TODO: This is incorrect, since we don't necessarily use all
				// of the points.
				fftCoeffs.set(mDim * j + i, 0, mag / (double) numPoints);
			}
		}
		return fftCoeffs;
	}

	/**
	 * Gets the dot product of two matrices.
	 * 
	 * @param m1
	 *            Matrix 1
	 * @param m2
	 *            Matrix 2
	 * @return The dot product of the two given matrices.
	 */
	public static double dot(final Matrix m1, final Matrix m2) {
		final int rDim = m1.getRowDimension();
		final int cDim = m1.getColumnDimension();
		double sum = 0;
		for (int r = 0; r < rDim; r++) {
			for (int c = 0; c < cDim; c++) {
				sum += m1.get(r, c) * m2.get(r, c);
			}
		}
		return sum;
	}

	/**
	 * Gives the dot product of two Measurements.
	 * 
	 * @param m1
	 *            Measurement 1
	 * @param m2
	 *            Measurement 2
	 * @return The dot product of the two given measurements
	 */
	public static float dot(final AccelSample m1, final AccelSample m2) {
		return (float) (m1.x * m2.x + m1.y * m2.y + m1.z * m2.z);
	}

	/**
	 * Determines if the two matrices are equal.
	 * 
	 * @param a
	 *            A Matrix
	 * @param b
	 *            A Matrix
	 * @param delta
	 *            How close two doubles have to be to be equal.
	 * @return true if the two matrices are equal, false otherwise
	 */
	public static boolean matrixEqual(final Matrix a, final Matrix b,
			final double delta) {
		// Take care of null cases
		if (a == null && b == null) {
			return true;
		}
		if (a == null || b == null) {
			return false;
		}

		// Make sure same dimension
		if (a.getRowDimension() != b.getRowDimension()
				|| a.getColumnDimension() != b.getColumnDimension()) {
			return false;
		}

		// Element-wise comparison
		final int rowDim = a.getRowDimension();
		final int colDim = a.getColumnDimension();
		for (int r = 0; r < rowDim; r++) {
			for (int c = 0; c < colDim; c++) {
				if (Math.abs(a.get(r, c) - b.get(r, c)) > delta) {
					return false;
				}
			}
		}

		return true;
	}

	/**
	 * Converts a vector (Matrix) to a Measurement.
	 * 
	 * @param m
	 *            Matrix to convert
	 * @return Measurement with values from the Matrix and 0 in the t component.
	 */
	public static AccelSample matrixToMeasurement(final Matrix m) {
		return new AccelSample((float) m.get(0, 0), (float) m.get(1, 0),
				(float) m.get(2, 0), 0);
	}

	/**
	 * Returns the max x, y, and z-values within the given measurements.
	 * 
	 * @param measurements
	 *            Measurements (as vectors) for which to get the max values.
	 * @return A Matrix containing the maximum values of each component. Returns
	 *         null if the given Vector of measurements is empty.
	 */
	public static Matrix maxAccels(final Vector<Matrix> measurements) {
		// Make sure there are some measurements
		if (measurements.size() == 0) {
			return null;
		}

		// Get dimension of matrices
		final int rDim = measurements.get(0).getRowDimension();
		final int cDim = measurements.get(0).getColumnDimension();

		final Matrix maxes = measurements.get(0).copy();

		// Iteratively take maxes
		final Iterator<Matrix> iter = measurements.iterator();
		while (iter.hasNext()) {
			final Matrix m = iter.next();
			for (int r = 0; r < rDim; r++) {
				for (int c = 0; c < cDim; c++) {
					maxes.set(r, c, Math.max(maxes.get(r, c), m.get(r, c)));
				}
			}
		}

		return maxes;
	}

	/**
	 * Returns the measurement of maximum magnitude within the given
	 * measurement.
	 * 
	 * @param measurements
	 *            Measurements to get the max magnitude measurement for.
	 * @return The Measurement with the largest magnitude. Returns null if the
	 *         given Vector of measurements is empty.
	 */
	public static AccelSample maxMag(final Vector<AccelSample> measurements) {
		// Make sure there are some measurements
		if (measurements.size() == 0) {
			return null;
		}

		AccelSample maxMeasurement = new AccelSample(0, 0, 0);
		double maxMagnitude = -1;

		// Iteratively get max
		final Iterator<AccelSample> iter = measurements.iterator();
		while (iter.hasNext()) {
			final AccelSample m = iter.next();
			double magnitude = Math.sqrt(m.x * m.x + m.y * m.y + m.z * m.z);
			if (magnitude > maxMagnitude) {
				maxMagnitude = magnitude;
				maxMeasurement = m;
			}
		}
		return maxMeasurement;
	}

	/**
	 * Returns the measurement of maximum magnitude within the given
	 * measurement.
	 * 
	 * @param measurements
	 *            Measurements to get the max magnitude measurement for, given
	 *            as column vectors
	 * @return The Measurement with the largest magnitude. Returns null if the
	 *         given Vector of measurements is empty.
	 */
	public static Matrix maxMagMatrix(final Vector<Matrix> measurements) {
		// Make sure there are some measurements
		if (measurements.size() == 0) {
			return null;
		}

		// Dimension of each point.
		int mDim = measurements.get(0).getRowDimension();

		Matrix maxes = new Matrix(mDim, 1, 0);

		// Iteratively get max
		for (Matrix m : measurements) {
			for (int i = 0; i < mDim; i++) {
				if (Math.abs(m.get(i, 0)) > Math.abs(maxes.get(i, 0))) {
					maxes.set(i, 0, Math.abs(m.get(i, 0)));
				}
			}
		}
		return maxes;
	}

	/**
	 * Converts a Vector of Measurements to a Vector of Matrices.
	 * 
	 * @param measurements
	 *            Measurements to convert
	 * @return Vector of Matrices as column vectors with values from the
	 *         Measurements
	 */
	public static Vector<Matrix> measurementsToMatrices(
			final Vector<AccelSample> measurements) {
		final Vector<Matrix> newVectors = new Vector<Matrix>();
		final Iterator<AccelSample> iter = measurements.iterator();
		while (iter.hasNext()) {
			newVectors.add(MathLib.measurementToMatrix(iter.next()));
		}
		return newVectors;
	}

	/**
	 * Converts a Measurement to a vector.
	 * 
	 * @param m
	 *            Measurement to convert
	 * @return Matrix as a column vector with values from the Measurement
	 */
	public static Matrix measurementToMatrix(final AccelSample m) {
		final Matrix newMat = new Matrix(3, 1);
		newMat.set(0, 0, m.x);
		newMat.set(1, 0, m.y);
		newMat.set(2, 0, m.z);
		return newMat;
	}

	/**
	 * Returns the 'moments' moment of the given Measurements.
	 * 
	 * @param measurements
	 *            Vector of measurements (as vectors) of which we want to get
	 *            the moments.
	 * @param moment
	 *            The moment to get.
	 * @return A Matrix giving the moments of each coordinate.
	 */
	public static Matrix moment(final Vector<Matrix> measurements,
			final double moment) {
		// Get dimension of matrices
		final int rDim = measurements.get(0).getRowDimension();
		final int cDim = measurements.get(0).getColumnDimension();

		final Matrix moments = new Matrix(rDim, cDim);
		final int numMeasurements = measurements.size();

		// Merely average each measurement to the 'moment' power
		for (Matrix m : measurements) {
			for (int r = 0; r < rDim; r++) {
				for (int c = 0; c < cDim; c++) {
					moments.set(r, c, moments.get(r, c)
							+ Math.pow(m.get(r, c), moment));
				}
			}
		}

		// Divide out by num_measurements to finish the averaging
		moments.timesEquals(1 / (double) numMeasurements);

		return moments;
	}

	/**
	 * Returns the 'moments' moment of the given Measurements, which must only
	 * have one column each.
	 * 
	 * @param measurements
	 *            Vector of measurements (as vectors) of which we want to get
	 *            the moments.
	 * @param moment
	 *            The moment to get.
	 * @return A Matrix giving the moments of each coordinate.
	 */
	public static Matrix momentVector(final Vector<Matrix> measurements,
			final double moment) {
		// Get dimension of matrices
		final int rDim = measurements.get(0).getRowDimension();
		final int cDim = measurements.get(0).getColumnDimension();

		final Matrix moments = new Matrix(rDim, cDim);
		final int numMeasurements = measurements.size();

		// Merely average each measurement to the 'moment' power
		for (Matrix m : measurements) {
			for (int r = 0; r < rDim; r++) {
				moments.set(r, 0, moments.get(r, 0)
						+ Math.pow(m.get(r, 0), moment));
			}
		}

		// Divide out by num_measurements to finish the averaging
		moments.timesEquals(1 / (double) numMeasurements);

		return moments;
	}

	/**
	 * Returns the mean of the given Measurements, which must only have one
	 * column each.
	 * 
	 * @param measurements
	 *            Vector of measurements (as vectors) of which we want to get
	 *            the moments.
	 * @return A Matrix giving the average of each coordinate.
	 */
	public static Matrix averageVectors(final Vector<Matrix> measurements) {
		// Get dimension of matrices
		final int rDim = measurements.get(0).getRowDimension();
		final int cDim = measurements.get(0).getColumnDimension();

		final Matrix average = new Matrix(rDim, cDim);
		final int numMeasurements = measurements.size();

		// Merely average each measurement to the 'moment' power
		for (Matrix m : measurements) {
			average.plusEquals(m);
		}

		// Divide out by num_measurements to finish the averaging
		average.timesEquals(1 / (double) numMeasurements);

		return average;
	}

	/**
	 * Returns the multivariate normal pdf value of the given point, when also
	 * given the mean vector, coefficient (multiplier) in the pdf formula, and
	 * inverse of the covariance matrix.
	 * 
	 * @param x
	 *            A column vector specifying the value at which to get the pdf.
	 * @param mean
	 *            Column vector giving the mean of the multivarate Gaussian for
	 *            which the pdf is being calculated.
	 * @param mult
	 *            Multiplier in front of the exponential in the formula for a
	 *            multivariate Gaussian.
	 * @param covInv
	 *            Inverse of the covariance matrix.
	 * @return The PDF value.
	 */
	public static double multivariatePDF(final Matrix x, final Matrix mean,
			final double mult, final Matrix covInv) {
		return mult
				* Math.exp(-.5
						* (x.minus(mean).transpose().times(covInv).times(x
								.minus(mean))).get(0, 0));
	}

	/**
	 * Returns the multivariate normal pdf value of the given point, when also
	 * given the mean vector and covariance matrix.
	 * 
	 * @param x
	 *            A column vector specifying the value at which to get the pdf.
	 * @param mean
	 *            Column vector giving the mean of the multivariate Gaussian for
	 *            which the pdf is being calculated.
	 * @param cov
	 *            Covariance matrix of the multivariate Gaussian for which the
	 *            pdf is being calculated.
	 * @return Value of the multivariate Gaussian pdf with the given parameters
	 *         at the given point.
	 */
	public static double multivariatePDF(final Matrix x, final Matrix mean,
			final Matrix cov) {
		final int dim = x.getRowDimension();
		return 1.
				/ (Math.pow(2 * Math.PI, dim / 2.) * Math.sqrt(cov.det()))
				* Math.exp(-.5
						* (x.minus(mean).transpose().times(cov.inverse())
								.times(x.minus(mean))).get(0, 0));
	}

	/**
	 * Returns the multivariate normal pdf value of the given point, when also
	 * given the mean vector and the inverse and determinant of the covariance
	 * matrix.
	 * 
	 * @param x
	 *            A column vector specifying the value at which to get the pdf.
	 * @param mean
	 *            Column vector giving the mean of the multivariate Gaussian for
	 *            which the pdf is being calculated.
	 * @param covInv
	 *            Inverse of the covariance matrix.
	 * @param covDet
	 *            Determinant of the covariance matrix.
	 * @return Value of the multivariate Gaussian pdf with the given parameters
	 *         at the given point.
	 */
	public static double multivariatePDF(final Matrix x, final Matrix mean,
			final Matrix covInv, final double covDet) {
		final int dim = x.getRowDimension();
		if (dim != mean.getRowDimension() || dim != covInv.getRowDimension()) {
			System.out.println("x dim: " + x.getRowDimension());
			System.out.println("mean dim: " + mean.getRowDimension());
			System.out.println("covInv dim: " + covInv.getRowDimension() + " "
					+ covInv.getColumnDimension());
		}
		return 1.
				/ (Math.pow(2 * Math.PI, dim / 2.) * Math.sqrt(covDet))
				* Math.exp(-.5
						* (x.minus(mean).transpose().times(covInv).times(x
								.minus(mean))).get(0, 0));
	}

	public static double multivariateLogPDF(final Matrix x, final Matrix mean,
			final Matrix covInv, final double covDet) {
		final int dim = x.getRowDimension();
		if (dim != mean.getRowDimension() || dim != covInv.getRowDimension()) {
			System.out.println("x dim: " + x.getRowDimension());
			System.out.println("mean dim: " + mean.getRowDimension());
			System.out.println("covInv dim: " + covInv.getRowDimension() + " "
					+ covInv.getColumnDimension());
		}
		return Math.log(1. / (Math.pow(2 * Math.PI, dim / 2.) * Math
				.sqrt(covDet)))
				+ -.5
				* (x.minus(mean).transpose().times(covInv).times(x.minus(mean)))
						.get(0, 0);
	}

	/**
	 * Normalizes the given Measurement, returning a new Measurement. Doesn't
	 * normalize in the t component.
	 * 
	 * @param m
	 *            Measurement to normalize
	 * @return A normalized version of the Measurement.
	 */
	public static AccelSample normalizeMeasurement(final AccelSample m) {
		final float mag = (float) Math.sqrt(MathLib.dot(m, m));
		return new AccelSample(m.x / mag, m.y / mag, m.z / mag, m.t);
	}

	/**
	 * Does PCA using a pre-computed PCA transformation.
	 * 
	 * @param input
	 *            The input to run PCA on. Has dimensions m by n, where m is the
	 *            number of dimensions of each point and n is the number of
	 *            points. Should already be Z-transformed
	 * @param wt
	 *            W transpose, calculated with pcaWTrans.
	 * @return A matrix giving the reduced-dimension features. The dimensions of
	 *         the reduced matrix are k by n, where k is the row dimension of
	 *         'wt', which is the number of dimensions that PCA is reducing to,
	 *         and the column dimension is n, the number of points.
	 */
	public static Matrix pcaFeature(final Matrix input, final Matrix wt) {
		return wt.times(input);
	}

	/**
	 * Does PCA using a precomputed PCA transformation.
	 * 
	 * @param input
	 *            The input to run PCA on. Has dimensions m by n, where m is the
	 *            number of dimensions of each point and n is the number of
	 *            points.
	 * @param c
	 *            Coefficients matrix computed from PCA. Has dimensions m by m.
	 * @param mean
	 *            Mean vector describing the average point
	 * @param sigma
	 *            Component-wise standard deviation vector for a point.
	 * @param k
	 *            Number of dimensions to retain after PCA
	 * @return A k by n matrix giving the reduced-dimension features
	 */
	public static Matrix pcaFeature(final Matrix input, final Matrix c,
			final Matrix mean, final Matrix sigma, final int k) {
		// First z-transform inputs
		final Matrix inputCopy = input.copy();
		MathLib.zTransform(inputCopy, mean, sigma);

		// Apply PCA transformation
		// Need to transpose as PCA used row-vectors as data points
		final Matrix features = inputCopy.transpose().times(c);

		// Return the first k components
		return features.getMatrix(1, features.getRowDimension(), 1, k);
	}

	/**
	 * Returns the matrix W^T, where the PCA transformation is given by Y^T =
	 * X^T * W, or equivalently, Y = W^T * X. W^T is truncated so that it only
	 * encompasses a 'varianceCaught' portion of the variance.
	 * 
	 * @param x
	 *            Input data to calculate the matrix for. Data points are given
	 *            by the columns of X. Should already be Z-transformed.
	 * @param varianceCaught
	 *            The portion of the total variance to capture. IN [0,1]
	 * @return W^T, the transpose of W in Y = W^T * X, the PCA decomposition of
	 *         X. // DOESN'T WORK
	 */
	public static Matrix pcaWTrans(final Matrix x, final double varianceCaught) {
		// First get W
		final SingularValueDecomposition svd = x.svd();
		final Matrix w = svd.getU(); // Called U in this library

		// Determine how many rows of W^T we need
		final double[] singularValues = svd.getSingularValues();
		// Get sum of singular values
		double sum = 0.;
		for (int i = 0; i < singularValues.length; i++) {
			sum += singularValues[i];
		}
		final double threshhold = sum * varianceCaught;
		// Sum until we reach the threshold
		double valSum = 0.;
		int catchIndex = -1;
		for (int i = 0; i < singularValues.length; i++) {
			valSum += singularValues[i];
			if (valSum >= threshhold) {
				catchIndex = i;
				break;
			}
		}

		// If we didn't reach the required variance caught, get everything
		if (catchIndex == -1) {
			catchIndex = singularValues.length - 1;
		}

		// Truncate the matrix and return
		final Matrix wt = w.transpose();
		return wt.getMatrix(0, catchIndex, 0, wt.getColumnDimension() - 1);
	}

	/**
	 * Constructs a rotation matrix for the rotation defined by the two given
	 * vectors.
	 * 
	 * @param v1
	 *            Vector to rotate from
	 * @param v2
	 *            Vector to rotate to
	 * @return Rotation matrix
	 */
	public static Matrix rotationMatrixVecVec(final Matrix v1, final Matrix v2) {
		// Make into Measurements so that everything looks much cleaner
		final AccelSample vec1 = MathLib.matrixToMeasurement(v1);
		final AccelSample vec2 = MathLib.matrixToMeasurement(v2);

		// First get axis of rotation and normalize it
		final AccelSample rotationAxis = new AccelSample(vec1.y * vec2.z
				- vec1.z * vec2.y, vec1.z * vec2.x - vec1.x * vec2.z, vec1.x
				* vec2.y - vec1.y * vec2.x, 0);
		final AccelSample ra = MathLib.normalizeMeasurement(rotationAxis);

		// Determine how much to rotate by getting the angle between the
		// two vectors.
		double dot = MathLib.dot(vec1, vec2);
		final double mag1 = Math.sqrt(MathLib.dot(vec1, vec1));
		final double mag2 = Math.sqrt(MathLib.dot(vec2, vec2));
		dot /= mag1;
		dot /= mag2;
		final double theta = Math.acos(dot);

		// Now make the matrix
		final double c = Math.cos(theta);
		final double s = Math.sin(theta);
		final Matrix rotationMatrix = new Matrix(3, 3);
		rotationMatrix.set(0, 0, ra.x * ra.x + (1 - ra.x * ra.x) * c);
		rotationMatrix.set(0, 1, ra.x * ra.y * (1 - c) - ra.z * s);
		rotationMatrix.set(0, 2, ra.x * ra.z * (1 - c) + ra.y * s);
		rotationMatrix.set(1, 0, ra.y * ra.x * (1 - c) + ra.z * s);
		rotationMatrix.set(1, 1, ra.y * ra.y + (1 - ra.y * ra.y) * c);
		rotationMatrix.set(1, 2, ra.y * ra.z * (1 - c) - ra.x * s);
		rotationMatrix.set(2, 0, ra.z * ra.x * (1 - c) - ra.y * s);
		rotationMatrix.set(2, 1, ra.z * ra.y * (1 - c) + ra.x * s);
		rotationMatrix.set(2, 2, ra.z * ra.z + (1 - ra.z * ra.z) * c);

		return rotationMatrix;

	}

	/**
	 * Sets a submatrix of 'toSet' using values from 'valuesFrom'.
	 * 
	 * @param toSet
	 *            Matrix to set a submatrix of
	 * @param r0
	 *            Row of top-left portion of submatrix to set, inclusive.
	 * @param c0
	 *            Column of top-left portion of submatrix to set, inclusive.
	 * @param r1
	 *            Row of bottom-right portion of submatrix to set, inclusive.
	 * @param c1
	 *            Column of bottom-right portion of submatrix to set, inclusive.
	 * @param valuesFrom
	 *            Matrix to copy from.
	 */
	public static void setSubMatrix(final Matrix toSet, final int r0,
			final int c0, final int r1, final int c1, final Matrix valuesFrom) {
		assert (valuesFrom.getRowDimension() == (r1 - r0 + 1));
		assert (valuesFrom.getColumnDimension() == (c1 - c0 + 1));
		for (int r = r0; r < r1 + 1; r++) {
			for (int c = c0; c < c1 + 1; c++) {
				toSet.set(r, c, valuesFrom.get(r - r0, c - c0));
			}
		}
	}

	/**
	 * Gets the standard deviation of each element within the given sample. Note
	 * that this is the sample standard deviation (divide by n), not an
	 * estimator for the population standard deviation (divide by n - 1).,
	 * 
	 * @param data
	 *            Each Matrix is another piece of data.
	 * @return A Matrix consisting of the sample standard deviation of each
	 *         component.
	 */
	public static Matrix standardDeviations(final Vector<Matrix> data) {
		Matrix mean = MathLib.moment(data, 1);
		int rows = mean.getRowDimension();
		int cols = mean.getColumnDimension();
		// Sum of difference squared from the true mean.
		Matrix sumSquareDiffs = new Matrix(rows, cols, 0);
		Iterator<Matrix> iter = data.iterator();
		while (iter.hasNext()) {
			Matrix m = iter.next();
			m.minusEquals(mean);
			// Square each element
			for (int r = 0; r < rows; r++) {
				for (int c = 0; c < cols; c++) {
					m.set(r, c, m.get(r, c) * m.get(r, c));
				}
			}
			sumSquareDiffs.plusEquals(m);
		}
		// Divide by the number of samples and take the square root
		int numSamples = data.size();
		for (int r = 0; r < rows; r++) {
			for (int c = 0; c < cols; c++) {
				sumSquareDiffs.set(r, c, Math.sqrt(sumSquareDiffs.get(r, c)
						/ numSamples));
			}
		}
		return sumSquareDiffs;
	}

	/**
	 * Normalizes the given input matrix, viewed as a series of vectors with
	 * mean 'mean' and component-wise standard deviation 'sigma'. Normalizes so
	 * that each component has mean 0 and standard deviation 1.
	 * 
	 * @param input
	 *            The data to be transformed. Transforms data in-place
	 * @param mean
	 *            A mean vector.
	 * @param sigma
	 *            Component-wise standard deviation vector.
	 */
	public static void zTransform(final Matrix input, final Matrix mean,
			final Matrix sigma) {
		for (int i = 0; i < input.getRowDimension(); i++) {
			for (int j = 0; j < input.getColumnDimension(); j++) {
				input.set(i, j, (input.get(i, j) - mean.get(i, j))
						/ sigma.get(i, j));
			}
		}
	}

	/**
	 * Returns the portion of the given vector from index 'start', inclusive, to
	 * index 'stop', exclusive. Zero-indexed. Slow, so try not to use this.
	 * 
	 * @param vec
	 *            Vector to get a subvector of.
	 * @param start
	 *            Starting index.
	 * @param stop
	 *            Stopping index.
	 * @return The subvector.
	 */
	public static Vector<AccelSample> vectorSubVec(
			final Vector<AccelSample> vec, final int start, final int stop) {
		// Just copy over the elements we want.
		final Vector<AccelSample> newMeas = new Vector<AccelSample>();
		for (int i = start; i < stop; i++) {
			newMeas.add(vec.get(i));
		}
		return newMeas;
	}

}
